/*
 *  commonutil.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 15/11/07.
 *
 */

#include "commonutil.h"


void addSystemName(string& str, const string& strname)	{ str = strname + "/" + str; }

string getStringHead(const string& str) { return(str.substr(0,str.find_last_of("/"))); }

string getStringTail(const string& str)	{ return(str.substr(str.find_last_of("/")+1)); }

string getStringExtension(const string& str)	{ return(str.substr(str.find_last_of(".")+1)); }

bool checkForSlash(const string& str)	{ if(str.find_last_of("/")>=str.size()) return(false); else return(true); }

string subsSlash(const string& str)
{
	string s1 = getStringHead(str);
	string s2 = getStringTail(str);
	return(s1+"-"+s2);
}

double lTwoNorm(const vector<double>& v1)
{	
	int i=0;
	double result=0.0;
	for(i=0; i<v1.size(); i++) result+= v1[i]*v1[i];
	return sqrt(result);
}

double lTwoNorm(const vector<double>& v1, const int& dim)
{	
	//if(fmod( double(v.size()),dim)!=0.0)
	//{
	//	cout << "*ERROR: Mismatch computing the lTwoNorm of a vector \n";
	//	return 0.0;
	//}
	//else
	//{
		int i=0;
		double result=0.0;
		for(i=0; i<v1.size()/2; i++) result+= (v1[i]-v1[i+dim])*(v1[i]-v1[i+dim]);
		return sqrt(result);
	//}
}

double lTwoNorm(const vector<double>& v1, const vector<double>& v2)
{	int i=0;
	double result=0.0;
	for(i=0; i<v1.size(); i++) result+= (v2[i]-v1[i])*(v2[i]-v1[i]);
	return sqrt(result); 
}

double lTwoNormSq(const vector<double>& v1)
{	
	int i=0;
	double result=0.0;
	for(i=0; i<v1.size(); i++) result+= v1[i]*v1[i];
	return result;
}

void uVersor(const vector<double>& v1, const vector<double>& v2, const double & dis, vector<double>& u)
{	int i;
	for(i=0; i<v1.size(); i++) u[i] = (v2[i]-v1[i])/dis;
}

void uVersor(const vector<double>& v1, const double & dis, const int & dim, vector<double>& u)
{	int i;
	for(i=0; i<v1.size()/2; i++) u[i] = (v1[i+dim]-v1[i])/dis;
}

void printSTLVector(vector<double>& v)
{
	int i;
	cout << "\n";
	for(i=0; i<v.size(); i++) cout << v[i] << "\n";
}

void printSTLVector(const vector<string>& v)
{
	int i;
	cout << "\n";
	for(i=0; i<v.size(); i++) cout << v[i] << "\n";
}

void printSTLVector(vector<int>& v)
{
	int i;
	cout << "\n";
	for(i=0; i<v.size(); i++) cout << v[i] << "\n";
}

void printEigenVectorXd(const VectorXd & v)
{
	int i;
	cout << "\n";
	for(i=0; i<v.rows(); i++) cout << v[i] << "\n";
}

void printSTLMatrix(const vector< vector<double> >& m)
{
	int i;
	int j;
	for(i=0; i<m.size(); i++)
	{
		for(j=0; j<m[0].size(); j++) 	
		{
			cout << m[i][j] << "\t" << "\t";
//			printf("%15.10f \t \t", m[i][j]);
		}
		cout << "\n";
	}
}

void printSTLMatrix(const vector< vector<int> >& m)
{
	int i;
	int j;
	for(i=0; i<m.size(); i++)
	{
		for(j=0; j<m[i].size(); j++) 	
		{
			cout << m[i][j] << "\t" << "\t";
		}
		cout << "\n";
	}
}

void printEigenMatrix3d(const Matrix3d& m)
{
	int i,j;

	for(i=0; i<3; i++)
	{
		for(j=0; j<3; j++) 	
		{
			cout << m(i,j) << "\t" << "\t";
//			printf("%15.10f \t \t", m(i,j));
		}
		cout << "\n";
	}
}

void printEigenMatrixXd(const MatrixXd& m)
{
	int i,j;
	int N,M;
	
	N = m.rows();
	M = m.cols();
	
	for(i=0; i<N; i++)
	{
		for(j=0; j<M; j++) 	
		{
			cout << m(i,j) << "\t" << "\t";
//			printf("%15.10f \t \t", m(i,j));
		}
		cout << "\n";
	}
}

void printSparseMatrixSLU(sparseMatrixSLU& m)
{
	int i;
	int j;
	for(i=0; i<m.n; i++)
	{
		for(j=0; j<m.n; j++) 	
		{
			cout << m.readElement(i,j) << " ";
		}
		cout << "\n";
	}
}

void printSparseMatrix(sparseMatrix& m)
{
	int i;
	int j;
	for(i=0; i<m.n; i++)
	{
		for(j=0; j<m.n; j++) 	
		{
			cout << m.readElement(i,j) << " ";
		}
		cout << "\n";
	}	
}

void setZero(vector<double>& v)
{
	int i;
	for(i=0; i<v.size(); i++) v[i]=0.0;
}

void setZero(vector< vector<double> >& m)
{
	int i;
	int j;
	for(i=0; i<m.size(); i++)
			for(j=0; j<m[0].size(); j++) m[i][j]=0.0;
}

// ****

void lu( vector<vector<double> >& mat )
{
	int i;
	int j;
	int k;
	double mult;
	int dim =  mat.size();
	double aux;
	
	for (k = 0; k<dim-1; k++)
	{
		for (i = k+1; i<dim; i++)
		{
			aux = mat[i][k];
			if( aux != 0 )
			{
				mult = aux / mat[k][k];
				mat[i][k] = mult;
				for (j = k+1; j<dim; j++)
				{
					mat[i][j] -= mult*mat[k][j];
				}
			}
		}
	}
}

// ****
//Arma
/*
void lu( mat & matriz )
{

	int k;
	int dim =  matriz.n_rows;
	
	for (k = 0; k<dim-1; k++)
	{
		matriz.submat( span(k+1,dim-1),span(k,k) ) /= matriz(k,k);
		matriz.submat( span(k+1,dim-1), span(k+1,dim-1) ) -= matriz.submat( span(k+1,dim-1), span(k,k) )*matriz.submat( span(k,k),span(k+1,dim-1) );
	}
}
*/
// ****

void forSub(vector<vector<double> >& mat, vector<double>& c)
{
	int i;
	int j;
	int dim =  mat.size();
	double aux;
	
	for (i = 1; i<dim; i++)
		for(j = 0; j < i; j++)
		{
			aux = mat[i][j];
			if (aux!=0.0) c[i] -= aux * c[j];
		}
}

// ****
//Arma
/*
void forSub(mat & matriz, rowvec & c)
{
	int i;
	int j;
	int dim =  matriz.n_rows;
	double aux;
	for (i = 1; i<dim; i++)
	{
		for(j = 0; j < i; j++)
		{
			aux = matriz(i,j);
			if (aux!=0.0) c(i) -= aux * c(j);
		}
	}
}
*/
// ****

void forSub(vector<vector<double> >& mat, vector<vector<double> >& mat_r)
{
	int i;
	int j;
	int dim =  mat.size();
	int dim_r = mat_r[0].size();
	vector<double> b(dim);
	for (j=0; j<dim_r; ++j)
	{
		for (i=0; i<dim; ++i)	b[i] = mat_r[i][j];
		forSub (mat, b);
		for (i=0; i<dim; ++i)	mat_r[i][j] = b[i];
	}
}

// ****

void forSubTrans(vector<vector<double> >& mat, vector<vector<double> >& mat_r)
{

	for (int i=0; i<mat_r.size(); ++i)
	{
		forSub (mat, mat_r[i]);
	}
}

// ****
//Arma
/*
void forSubTrans(mat & matriz, mat & mat_r)
{
	rowvec vaux;
	for (int i=0; i<mat_r.n_rows; ++i)
	{
		vaux = mat_r.row(i);
		forSub (matriz, vaux);
		mat_r.row(i) = vaux;
	}
}
*/
// ****

void backSub(vector<vector<double> >& mat, vector<double>& c)
{
	int j;
	int k;
	int dim =  mat.size();
	double aux;
	
	c[dim-1] /= mat[dim-1][dim-1];
	
	for (k=dim-2; k>=0; --k)
	{
		for (j = k+1; j<dim; ++j)
		{
			aux = mat[k][j];
			if (aux!=0.0) c[k] -= aux*c[j] ;
		}
		c[k] /= mat[k][k];
	}
}

// ****
//Arma
/*
void backSub(mat & matriz, rowvec & c)
{
	int j;
	int k;
	int dim =  matriz.n_rows;
	double aux;
	
	c(dim-1) /= matriz(dim-1,dim-1);
	
	for (k=dim-2; k>=0; --k)
	{
		for (j = k+1; j<dim; ++j)
		{
			aux = matriz(k,j);
			if (aux!=0.0) c(k) -= aux*c(j) ;
		}
		c(k) /= matriz(k,k);
	}
}
*/
// ****

void backSub(vector<vector<double> >& mat, vector<vector<double> >& mat_r)
{
	int i;
	int j;
	int dim =  mat.size();
	int dim_r = mat_r[0].size();
	vector<double> b(dim);
	for (j=0; j<dim_r; ++j)
	{
		for (i=0; i<dim; ++i)	b[i] = mat_r[i][j];
		backSub (mat, b);
		for (i=0; i<dim; ++i)	mat_r[i][j] = b[i];
	}
}

// ****

void backSubTrans(vector<vector<double> >& mat, vector<vector<double> >& mat_r)
{
	for (int i=0; i<mat_r.size(); ++i)
	{
		backSub (mat, mat_r[i]);
	}
}

// ****
//Arma
/*
void backSubTrans(mat & matriz, mat & mat_r)
{
	rowvec vaux;
	for (int i=0; i<mat_r.n_rows; ++i)
	{
		vaux = mat_r.row(i);
		backSub (matriz, vaux);
		mat_r.row(i) = vaux;
	}
}
*/
// ****

int locateInSparse(vector<int>& ia, vector<int>& ja, int& I, int& J)
{
	int i;
	int pos = -1;
	for (i=ja[J]; i<ja[J+1]; ++i)
	{
		if(ia[i-1]==I+1)
		{
			pos = i-1;
			break;
		}
	}
	return pos;	
}

// ****

void inverse(vector<vector<double> >& mat)
{
	int i;
	int j;
	vector<double> aux(mat.size());
	vector<vector<double> > matInv;
	matInv = mat;
	
	lu(mat);
	for (j=0; j<mat.size(); ++j)
	{
		setZero(aux);
		aux[j] = 1.0;
		forSub(mat,aux);
		backSub(mat,aux);
		for (i=0; i<mat.size(); ++i)
		{
			matInv[i][j] = aux[i];
		}
	}
	mat = matInv;
}

// ****

void rigidTransOver(vector<double>& x, vector<double>& x0, vector<vector<double> >& R)
{
	// Active rotation
	// x <- x0 + R*x
	// x is overwritten
	
	int i;
	int j;
	vector<double> aux(x.size());
	
	for (i=0; i<x.size(); ++i)
	{
		for (j=0; j<x.size(); ++j)
		{
			aux[i]+= R[i][j]*x[j];
		}
	}
	for (i=0; i<x.size(); ++i)	x[i] = x0[i] + aux[i];
}

// ****

void rigidTrans(vector<double>& x, vector<double>& x0, vector<vector<double> >& R, vector<double>& aux)
{
	// Active rotation
	// x <- x0 + R*x
	// x is NOT overwritten
	
	int i;
	int j;
	setZero(aux);
	
	for (i=0; i<x.size(); ++i)
	{
		for (j=0; j<x.size(); ++j)
		{
			aux[i]+= R[i][j]*x[j];
		}
	}
	for (i=0; i<x.size(); ++i)	aux[i]+= x0[i];
}

// ****

void getGlobalCoords(vector<double>& q, vector<double>& localCoords, vector<double>& globalCoords)
{
	// Assumed that q contains the coordinates of 4 points that define an orthonormal base
	// First point is the origin of base
	// r = r0 + rho
	// r0[i] = q[i] for i=0,1,2
	// rho = alpha* (r1-r0) + beta*(r2-r0) + gamma*(r3-r0)
	
	globalCoords[0] = q[0];
	globalCoords[0]+= localCoords[0]*(q[3]-q[0]);
	globalCoords[0]+= localCoords[1]*(q[6]-q[0]);
	globalCoords[0]+= localCoords[2]*(q[9]-q[0]);
	
	globalCoords[1] = q[1];
	globalCoords[1]+= localCoords[0]*(q[4]-q[1]);
	globalCoords[1]+= localCoords[1]*(q[7]-q[1]);
	globalCoords[1]+= localCoords[2]*(q[10]-q[1]);
	
	globalCoords[2] = q[2];
	globalCoords[2]+= localCoords[0]*(q[5]-q[2]);
	globalCoords[2]+= localCoords[1]*(q[8]-q[2]);
	globalCoords[2]+= localCoords[2]*(q[11]-q[2]);	
}

// ****

void getLocalCoords(vector<double>& q, vector<double>& localCoords, vector<double>& globalCoords)
{
	// Assumed that q contains the coordinates of 4 points that define an orthonormal base
	// First point is the origin of base
	// alpha = [ (r-r0)*(r1-r0) ] / (r1-r0)^2  (with (r1-r0)^2=1)
	// alpha = (r-r0)*(r1-r0)
	// beta =  (r-r0)*(r2-r0)
	// gamma = (r-r0)*(r3-r0)
	
	// x,y,x are the coordinates of (r-r0)
	double x = globalCoords[0]-q[0];
	double y = globalCoords[1]-q[1];
	double z = globalCoords[2]-q[2];
	
	localCoords[0] = x*(q[3]-q[0]);
	localCoords[0]+= y*(q[4]-q[1]);
	localCoords[0]+= z*(q[5]-q[2]);
	
	localCoords[1] = x*(q[6]-q[0]);
	localCoords[1]+= y*(q[7]-q[1]);
	localCoords[1]+= z*(q[8]-q[2]);
	
	localCoords[2] = x*(q[9]-q[0]);
	localCoords[2]+= y*(q[10]-q[1]);
	localCoords[2]+= z*(q[11]-q[2]);
	
}

// ****

void getGlobalCoordsNoTrans(vector<double>& q, vector<double>& localCoords, vector<double>& globalCoords)
{
	// Assumed that q contains the coordinates of 4 points that define an orthonormal base
	// First point is the origin of base
	// r = alpha* (r1-r0) + beta*(r2-r0) + gamma*(r3-r0)
	
	globalCoords[0] = localCoords[0]*(q[3]-q[0]);
	globalCoords[0]+= localCoords[1]*(q[6]-q[0]);
	globalCoords[0]+= localCoords[2]*(q[9]-q[0]);
	
	globalCoords[1] = localCoords[0]*(q[4]-q[1]);
	globalCoords[1]+= localCoords[1]*(q[7]-q[1]);
	globalCoords[1]+= localCoords[2]*(q[10]-q[1]);
	
	globalCoords[2] = localCoords[0]*(q[5]-q[2]);
	globalCoords[2]+= localCoords[1]*(q[8]-q[2]);
	globalCoords[2]+= localCoords[2]*(q[11]-q[2]);	
}

// ****

void substractSTLVectors(vector<double>& a, vector<double>& b)
{
	int i;
	if (a.size()!=b.size())	cout << "*ERROR: trying to substract vectors of different size" << endl;
	for (i=0; i<a.size(); ++i)	a[i]-=b[i];
}

// ****

void addSTLVectors(vector<double>& a, vector<double>& b)
{
	int i;
	if (a.size()!=b.size())	cout << "*ERROR: trying to add vectors of different size" << endl;
	for (i=0; i<a.size(); ++i)	a[i]+=b[i];
}

// ****

void crossProdSTLVectors(vector<double>& a, vector<double>& b)
{
	int i;
	if (a.size()!=b.size())	cout << "*ERROR: trying to substract vectors of different size" << endl;
	if (a.size()<3)	cout << "*ERROR: trying to perform a cross product in dimension less than 3" << endl;
	vector<double> aux(a.size());
	aux[0] = a[1]*b[2] - a[2]*b[1];
	aux[1] = a[2]*b[0] - a[0]*b[2];
	aux[2] = a[0]*b[1] - a[1]*b[0];
	b = aux;
}

// ****

void evalInBeta(double& beta, vector<double>& vBeta, vector<double>& vN, vector<double>& vNPlus)
{
	int i;
	for (i=0; i<vN.size(); ++i)
	{
		vBeta[i] = vN[i] + beta*( vNPlus[i]-vN[i] );
	}
}

// ****

void evalMid(vector<double>& vMid, vector<double>& vN, vector<double>& vNPlus)
{
	int i;
	for (i=0; i<vN.size(); ++i)
	{
		vMid[i] = ( vNPlus[i] + vN[i] ) / 2.0;
	}
}

//***

void VoigtFormat(const vector< vector < double > >& T2, vector < double > & T2v)
{
	T2v[0] = T2[0][0];
	T2v[1] = T2[1][1];
	T2v[2] = T2[2][2];
	T2v[3] = T2[0][1];
	T2v[4] = T2[0][2];
	T2v[5] = T2[1][2];
}

//***

void VoigtFormat(const Tensor2 & T2, vector < double > & T2v)
{
	T2v[0] = T2(1,1);
	T2v[1] = T2(2,2);
	T2v[2] = T2(3,3);
	T2v[3] = T2(2,1);
	T2v[4] = T2(3,1);
	T2v[5] = T2(3,2);
}

//***

void VoigtFormat(const Tensor2 & T2, VectorXd & T2v)
{
	T2v(0) = T2(1,1);
	T2v(1) = T2(2,2);
	T2v(2) = T2(3,3);
	T2v(3) = T2(2,1);
	T2v(4) = T2(3,1);
	T2v(5) = T2(3,2);
}

void VoigtFormat(const Matrix3d & T2, VectorXd & T2v)
{
	T2v(0) = T2(0,0);
	T2v(1) = T2(1,1);
	T2v(2) = T2(2,2);
	T2v(3) = T2(0,1);
	T2v(4) = T2(0,2);
	T2v(5) = T2(1,2);
}

//***

void VoigtFormat(const vector< vector < vector < vector < double > > > >& T4, vector < vector < double > > & T4v)
{
	T4v[0][0] = T4[0][0][0][0];
	T4v[0][1] = T4[0][0][1][1];
	T4v[0][2] = T4[0][0][2][2];
	T4v[0][3] = T4[0][0][0][1];
	T4v[0][4] = T4[0][0][0][2];
	T4v[0][5] = T4[0][0][1][2];
	
	
	T4v[1][0] = T4v[0][1];
	T4v[1][1] = T4[1][1][1][1];
	T4v[1][2] = T4[1][1][2][2];
	T4v[1][3] = T4[1][1][0][1];
	T4v[1][4] = T4[1][1][0][2];
	T4v[1][5] = T4[1][1][1][2];
	
	T4v[2][0] = T4v[0][2];
	T4v[2][1] = T4v[1][2];
	T4v[2][2] = T4[2][2][2][2];
	T4v[2][3] = T4[2][2][0][1];
	T4v[2][4] = T4[2][2][0][2];
	T4v[2][5] = T4[2][2][1][2];
	
	T4v[3][0] = T4v[0][3];
	T4v[3][1] = T4v[1][3];
	T4v[3][2] = T4v[2][3];
	T4v[3][3] = T4[0][1][0][1];
	T4v[3][4] = T4[0][1][0][2];
	T4v[3][5] = T4[0][1][1][2];
	
	T4v[4][0] = T4v[0][4];
	T4v[4][1] = T4v[1][4];
	T4v[4][2] = T4v[2][4];
	T4v[4][3] = T4v[3][4];
	T4v[4][4] = T4[0][2][0][2];
	T4v[4][5] = T4[0][2][1][2];
	
	T4v[5][0] = T4v[0][5];
	T4v[5][1] = T4v[1][5];
	T4v[5][2] = T4v[2][5];
	T4v[5][3] = T4v[3][5];
	T4v[5][4] = T4v[4][5];
	T4v[5][5] = T4[1][2][1][2];
}

//***

void VoigtFormat(const Tensor4 & T4, vector < vector < double > > & T4v)
{
  	T4v[0][0] = T4(1,1,1,1);
	T4v[0][1] = T4(1,1,2,2);
	T4v[0][2] = T4(1,1,3,3);
	T4v[0][3] = T4(1,1,2,1);
	T4v[0][4] = T4(1,1,3,1);
	T4v[0][5] = T4(1,1,3,2);
	
	T4v[1][0] = T4v[0][1];
	T4v[1][1] = T4(2,2,2,2);
	T4v[1][2] = T4(2,2,3,3);
	T4v[1][3] = T4(2,2,2,1);
	T4v[1][4] = T4(2,2,3,1);
	T4v[1][5] = T4(2,2,3,2);
	
	T4v[2][0] = T4v[0][2];
	T4v[2][1] = T4v[1][2];
	T4v[2][2] = T4(3,3,3,3);
	T4v[2][3] = T4(3,3,2,1);
	T4v[2][4] = T4(3,3,3,1);
	T4v[2][5] = T4(3,3,3,2);
	
	T4v[3][0] = T4v[0][3];
	T4v[3][1] = T4v[1][3];
	T4v[3][2] = T4v[2][3];
	T4v[3][3] = T4(2,1,2,1);
	T4v[3][4] = T4(2,1,3,1);
	T4v[3][5] = T4(2,1,3,2);
	
	T4v[4][0] = T4v[0][4];
	T4v[4][1] = T4v[1][4];
	T4v[4][2] = T4v[2][4];
	T4v[4][3] = T4v[3][4];
	T4v[4][4] = T4(3,1,3,1);
	T4v[4][5] = T4(3,1,3,2);
	
	T4v[5][0] = T4v[0][5];
	T4v[5][1] = T4v[1][5];
	T4v[5][2] = T4v[2][5];
	T4v[5][3] = T4v[3][5];
	T4v[5][4] = T4v[4][5];
	T4v[5][5] = T4(3,2,3,2);
}

//***

void VoigtFormat(const Tensor4 & T4, MatrixXd & T4v)
{
  	T4v(0,0) = T4(1,1,1,1);
	T4v(0,1) = T4(1,1,2,2);
	T4v(0,2) = T4(1,1,3,3);
	T4v(0,3) = T4(1,1,2,1);
	T4v(0,4) = T4(1,1,3,1);
	T4v(0,5) = T4(1,1,3,2);
	
	T4v(1,0) = T4v(0,1);
	T4v(1,1) = T4(2,2,2,2);
	T4v(1,2) = T4(2,2,3,3);
	T4v(1,3) = T4(2,2,2,1);
	T4v(1,4) = T4(2,2,3,1);
	T4v(1,5) = T4(2,2,3,2);
	
	T4v(2,0) = T4v(0,2);
	T4v(2,1) = T4v(1,2);
	T4v(2,2) = T4(3,3,3,3);
	T4v(2,3) = T4(3,3,2,1);
	T4v(2,4) = T4(3,3,3,1);
	T4v(2,5) = T4(3,3,3,2);
	
	T4v(3,0) = T4v(0,3);
	T4v(3,1) = T4v(1,3);
	T4v(3,2) = T4v(2,3);
	T4v(3,3) = T4(2,1,2,1);
	T4v(3,4) = T4(2,1,3,1);
	T4v(3,5) = T4(2,1,3,2);
	
	T4v(4,0) = T4v(0,4);
	T4v(4,1) = T4v(1,4);
	T4v(4,2) = T4v(2,4);
	T4v(4,3) = T4v(3,4);
	T4v(4,4) = T4(3,1,3,1);
	T4v(4,5) = T4(3,1,3,2);
	
	T4v(5,0) = T4v(0,5);
	T4v(5,1) = T4v(1,5);
	T4v(5,2) = T4v(2,5);
	T4v(5,3) = T4v(3,5);
	T4v(5,4) = T4v(4,5);
	T4v(5,5) = T4(3,2,3,2);
}

void InverseVoigtFormat(Matrix3d & Cf, const VectorXd & C)
{
	Cf(0,0) = C(0);
	Cf(1,1) = C(1);
	Cf(2,2) = C(2);
	Cf(0,1) = C(3);
	Cf(0,2) = C(4);
	Cf(1,2) = C(5);
	
	Cf = Cf.selfadjointView<Eigen::Upper>();
	
// 	Cf(1,0) = Cf(0,1);
// 	Cf(2,0) = Cf(0,2);
// 	Cf(2,1) = Cf(1,2);
}

void InverseVoigtFormat(Tensor2Sym & CT, const VectorXd & C)
{
	CT(1,1) = C(0);
	CT(2,2) = C(1);
	CT(3,3) = C(2);
	CT(2,1) = C(3);
	CT(3,1) = C(4);
	CT(3,2) = C(5);
}

//***

double dotVoigt(const VectorXd & V1, const VectorXd & V2)
{
	int n = V1.rows();
	int m = V2.rows();
	
	VectorXd hV1(n/2), hV2(n/2), tV1(m/2), tV2(m/2);
	
	if(n==m)
	{
		hV1 = V1.head(n / 2);
		hV2 = V2.head(n / 2);
		
		tV1 = V1.tail(n / 2);
		tV2 = V2.tail(n / 2);
	}
	else
	{
		cout << "ERROR: vectors size does not match" << endl;
	}
	return hV1.dot(hV2) + 2.0 * tV1.dot(tV2) ;
}

// ***

VectorXd makeVoigt(const VectorXd & C)
{
    VectorXd V(6);
    
    V.head(3) = C.head(3);
    V.tail(3) = 2.0 * C.tail(3);
    
    return V;
}

//***

VectorXd makeCiVoigt(const VectorXd & C)
{
    VectorXd Ci(6);
    Matrix3d CiSym, CSym;
    
    InverseVoigtFormat(CSym, C);
    
    CiSym = CSym.inverse();
    
    VoigtFormat(CiSym,Ci);
    
    return Ci;
}

// VectorXd dotVoigt( const MatrixXd & M1, const MatrixXd & M2)
// {
// 	int n1 = M1.rows();
// 	int m1 = M1.cols();
// 	
// 	int n2 = M2.rows();
// 	int m2 = M2.cols();
// 	
// 	
// 	VectorXd V(n1),hV1(n1/2), hV2(n1/2), tV1(n2/2), tV2(n2/2);
// 	
// 	if(n1==n2 && m1==m2)
// 	{
// 		for(int i=0;i<m1;++i)
// 		{
// 			hV1 = M1.col(i).head(n1 / 2);
// 			hV2 = M2.col(i).head(n1 / 2);
// 			tV1 = M1.col(i).tail(n2 / 2);
// 			tV2 = M2.col(i).tail(n2 / 2);
// 			
// 			V(i) = hV1.dot(hV2) + 2.0 * (tV1).dot(tV2) ;  
// 		}
// 	}
// 	else
// 	{
// 		cout << "ERROR: vectors size do not match" << endl;
// 	}
// 	return V;  
// }

// ***

SuperMatrix convert_stl_SLU(vector<vector<double> >& stlmatrix)
{	
	int i;
	int j;
	
	SuperMatrix slumatrix;
	
	int nrhs = stlmatrix.size();
	int n = stlmatrix[0].size();
	
	double *rhsb = (double *) doubleMalloc(n * nrhs );
	
	dCreate_Dense_Matrix(&slumatrix, n, nrhs, rhsb, n, SLU_DN, SLU_D, SLU_GE);	
	
	#pragma omp parallel for private (i)
	for(i=0; i<nrhs; i++)
	{
		for(j=0; j<n; j++)
		{
			rhsb[j+n*i] = stlmatrix[i][j];
		}
	}
	
	((DNformat*) slumatrix.Store)->nzval = rhsb;
	slumatrix.ncol = nrhs;
	
	return(slumatrix);
}

// ***

void convert_SLU_stl(SuperMatrix& slumatrix, vector<vector<double> >& stlmatrix)
{
	int i;
	int j;
	int nrhs = stlmatrix.size();
	int n = stlmatrix[0].size();
	
	double *rhsbX = (double *) doubleMalloc(n * nrhs );
	rhsbX = (double*) ((DNformat*) slumatrix.Store)->nzval;
	
	// WARNING This loop can not be parallelized
	for(i=0; i<nrhs; i++)
	{
		for(j=0; j<n; j++)
		{
			stlmatrix[i][j] = rhsbX[j+n*i];
		}
	}
}

// ***
